module utils.matrix;

import utils.vector3;
import std.math;

public struct Matrix3(T) {
    public alias Vector3!(T) MVector;
    public MVector[3] m;

    /+ Matrix:
        m[0].x1 m[1].x1 m[2].x1
        m[0].x2 m[1].x2 m[2].x2
        m[0].x3 m[1].x3 m[2].x3
    +/

    public static Matrix3 opCall(MVector.Type s) {
        Matrix3 r = void;
        //default: kind of unit matrix
        r.m[0] = MVector(s, 0, 0);
        r.m[1] = MVector(0, s, 0);
        r.m[2] = MVector(0, 0, s);
        return r;
    }

    public static Matrix3 opCall(MVector v0, MVector v1, MVector v2) {
        Matrix3 r = void;
        r.m[0] = v0;
        r.m[1] = v1;
        r.m[2] = v2;
        return r;
    }

    //xxx: inefficient implementation
    public void opMulAssign(in Matrix3 ma) {
        *this = this * ma;
    }

    public Matrix3 opMul(in Matrix3 ma) {
        Matrix3 r = void;

        for (int x = 0; x < 3; x++) {
            for (int y = 0; y < 3; y++) {
                float sum = 0.0f;
                for (int z = 0; z < 3; z++) {
                    sum += m[z][x] * ma.m[y][z];
                }
                r.m[y][x] = sum;
            }
        }
/*
        r.m[0].x1 = m[0].x1 * ma.m[0].x1 + m[1].x1 * ma.m[0].x2 + m[2].x1 * ma.m[0].x3;
        r.m[1].x1 = m[0].x1 * ma.m[1].x1 + m[1].x1 * ma.m[1].x2 + m[2].x1 * ma.m[1].x3;
        r.m[2].x1 = m[0].x1 * ma.m[2].x1 + m[1].x1 * ma.m[2].x2 + m[2].x1 * ma.m[2].x3;

        r.m[0].x2 = m[0].x2 * ma.m[0].x1 + m[1].x2 * ma.m[0].x2 + m[2].x2 * ma.m[0].x3;
        r.m[1].x2 = m[0].x2 * ma.m[1].x1 + m[1].x2 * ma.m[1].x2 + m[2].x2 * ma.m[1].x3;
        r.m[2].x2 = m[0].x2 * ma.m[2].x1 + m[1].x2 * ma.m[2].x2 + m[2].x2 * ma.m[2].x3;

        r.m[0].x3 = m[0].x3 * ma.m[0].x1 + m[1].x3 * ma.m[0].x2 + m[2].x3 * ma.m[0].x3;
        r.m[1].x3 = m[0].x3 * ma.m[1].x1 + m[1].x3 * ma.m[1].x2 + m[2].x3 * ma.m[1].x3;
        r.m[2].x3 = m[0].x3 * ma.m[2].x1 + m[1].x3 * ma.m[2].x2 + m[2].x3 * ma.m[2].x3;
*/
        return r;
    }

    public Matrix3 transpose() {
        Matrix3 r = void;
        r.m[0].x1 = m[0].x1;
        r.m[1].x2 = m[1].x2;
        r.m[2].x3 = m[2].x3;

        r.m[1].x1 = m[0].x2;
        r.m[2].x1 = m[0].x3;
        r.m[2].x2 = m[1].x3;

        r.m[0].x2 = m[1].x1;
        r.m[0].x3 = m[2].x1;
        r.m[1].x3 = m[2].x2;

        return r;
    }

    public MVector opMul(in MVector v) {
        MVector v2 = void;
        v2.x1 = m[0].x1 * v.x1 + m[1].x1 * v.x2 + m[2].x1 * v.x3;
        v2.x2 = m[0].x2 * v.x1 + m[1].x2 * v.x2 + m[2].x2 * v.x3;
        v2.x3 = m[0].x3 * v.x1 + m[1].x3 * v.x2 + m[2].x3 * v.x3;
        return v2;
    }

    public Matrix3 opMul(MVector.Type scalar) {
        Matrix3 tmp = *this;
        tmp *= scalar;
        return tmp;
    }
    public Matrix3 opMul_r(MVector.Type scalar) {
        return opMul(scalar);
    }
    public void opMulAssign(MVector.Type scalar) {
        m[0] *= scalar;
        m[1] *= scalar;
        m[2] *= scalar;
    }

    public Matrix3 opAdd(in Matrix3 v) {
        Matrix3 r = void;
        r.m[0] = m[0]+v.m[0];
        r.m[1] = m[1]+v.m[1];
        r.m[2] = m[2]+v.m[2];
        return r;
    }

    public void opAddAssign(in Matrix3 v) {
        m[0] = m[0]+v.m[0];
        m[1] = m[1]+v.m[1];
        m[2] = m[2]+v.m[2];
    }

    //http://www.ais.fhg.de/ARC/3D/download/diplom/node26.html
    public static Matrix3 fromEulerX(MVector.Type t) {
        return Matrix3(MVector(1,0,0),
                       MVector(0,cos(t),sin(t)),
                       MVector(0,-sin(t),cos(t)));
    }
    public static Matrix3 fromEulerY(MVector.Type t) {
        return Matrix3(MVector(cos(t),0,sin(t)),
                       MVector(0,1,0),
                       MVector(-sin(t),0,cos(t)));
    }
    public static Matrix3 fromEulerZ(MVector.Type t) {
        return Matrix3(MVector(cos(t),sin(t),0),
                       MVector(-sin(t),cos(t),0),
                       MVector(0,0,1));
    }

    //great fun
    public static Matrix3 fromAngles(in MVector a) {
        //http://www.gamedev.net/reference/articles/article1095.asp

        //Angles -> (unit) Quaternion
        auto syaw = sin(a.z/2.0f);
        auto spitch = sin(a.y/2.0f);
        auto sroll = sin(a.x/2.0f);
        auto cyaw = cos(a.z/2.0f);
        auto cpitch = cos(a.y/2.0f);
        auto croll = cos(a.x/2.0f);

        auto x = sroll * cpitch * cyaw - croll * spitch * syaw;
        auto y = croll * spitch * cyaw + sroll * cpitch * syaw;
        auto z = croll * cpitch * syaw - sroll * spitch * cyaw;
        auto w = croll * cpitch * cyaw + sroll * spitch * syaw;

        //(unit) Quaternion -> Matrix
        auto xx = 2*x*x; auto yy = 2*y*y; auto zz = 2*z*z;
        auto xy = 2*x*y; auto xz = 2*x*z; auto yz = 2*y*z;
        auto wx = 2*w*x; auto wy = 2*w*y; auto wz = 2*w*z;

        Matrix3 r = void;

        /*r.m[0].x1 = 1 - ( yy + zz );
        r.m[0].x2 = ( xy - wz );
        r.m[0].x3 = ( xz + wy );

        r.m[1].x1 = ( xy + wz );
        r.m[1].x2 = 1 - ( xx + zz );
        r.m[1].x3 = ( yz - wx );

        r.m[2].x1 = ( xz - wy );
        r.m[2].x2 = ( yz + wx );
        r.m[2].x3 = 1 - ( xx + yy );*/

        r.m[0].x1 = 1 - ( yy + zz );
        r.m[1].x1 = ( xy - wz );
        r.m[2].x1 = ( xz + wy );

        r.m[0].x2 = ( xy + wz );
        r.m[1].x2 = 1 - ( xx + zz );
        r.m[2].x2 = ( yz - wx );

        r.m[0].x3 = ( xz - wy );
        r.m[1].x3 = ( yz + wx );
        r.m[2].x3 = 1 - ( xx + yy );

        return r;
    }

    public Matrix3 inverse() {
        Matrix3 r = void;
        //wikipedia: det(A) = a11a22a33 + a12a23a31 + a13a21a32 - a13a22a31 - a11a23a32 - a12a21a33
        MVector.Type det = m[0].x1*m[1].x2*m[2].x3
                          +m[0].x1*m[1].x3*m[2].x1
                          +m[0].x2*m[1].x1*m[2].x2
                          -m[0].x3*m[1].x2*m[2].x1
                          -m[0].x1*m[1].x3*m[2].x2
                          -m[0].x2*m[1].x1*m[2].x3;
        auto det1 = 1.0f/det;

        //may the compiler optimize away all teh waste

        auto a = m[0].x1;
        auto b = m[1].x1;
        auto c = m[2].x1;
        auto d = m[0].x2;
        auto e = m[1].x2;
        auto f = m[2].x2;
        auto g = m[0].x3;
        auto h = m[1].x3;
        auto i = m[2].x3;

        r.m[0].x1 = det1*(e*i-f*h);
        r.m[0].x2 = det1*(f*g-d*i);
        r.m[0].x3 = det1*(d*h-e*g);
        r.m[1].x1 = det1*(c*h-b*i);
        r.m[1].x2 = det1*(a*i-c*g);
        r.m[1].x3 = det1*(b*g-a*h);
        r.m[2].x1 = det1*(b*f-c*e);
        r.m[2].x2 = det1*(c*d-a*f);
        r.m[2].x3 = det1*(a*e-b*d);

        return r;
    }

    public char[] toString() {
        return "[" ~ m[0].toString() ~ ", " ~ m[1].toString() ~ ", " ~
            m[2].toString() ~ "]";
    }
}

alias Matrix3!(float) Matrix3f;

